#include "StdAfx.h"
#include "LeanPoseModifier.h"

#include <CryExtension/CryCreateClassInstance.h>


CRYREGISTER_CLASS( CLeanPoseModifier )

static const uint32 INVALIDJOINTID = static_cast< uint32 >( -1 );

CLeanPoseModifier::CLeanPoseModifier()
{
	Clear();
}


CLeanPoseModifier::~CLeanPoseModifier()
{
}


void CLeanPoseModifier::Clear()
{
	for ( uint32 i = 0; i < ILeanPoseModifier::LeanJointCount; ++i )
	{
		m_additionalRotations[ i ] = Quat( IDENTITY );
	}

	for ( uint32 i = 0; i < ILeanPoseModifier::LeanJointCount; ++i )
	{
		m_jointIds[ i ] = INVALIDJOINTID;
	}
}


void CLeanPoseModifier::SetJointLeanAngle( const uint32 index, const uint32 jointId, const float leanAngleRadians )
{
	assert( index < ILeanPoseModifier::LeanJointCount );

	if ( jointId == INVALIDJOINTID )
	{
		return;
	}

	m_jointIds[ index ] = jointId;

	const Vec3 leanAxis( 0, 1, 0 );
	m_additionalRotations[ index ] = Quat::CreateRotationAA( leanAngleRadians, leanAxis );
}


SPU_INDIRECT(CommandBufferExecute(ML))
bool CLeanPoseModifier::Execute( const SAnimationPoseModiferParams& params )
{
	QuatT* pPoseRelative = SPU_LOCAL_PTR( params.pPoseRelative );

	for ( uint32 i = 0; i < ILeanPoseModifier::LeanJointCount; ++i )
	{
		uint32 jointId = m_jointIds[ i ];

		if(jointId != INVALIDJOINTID)
		{
				const Quat& additionalJointRotation = m_additionalRotations[ i ];

				QuatT& relJoint = pPoseRelative[ jointId ];
				relJoint = relJoint * QuatT( Vec3( IDENTITY ), additionalJointRotation );
		}
	}

	return true;
}